﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using System.IO;
using System.Threading;
using System.Collections;
using Microsoft.Xna.Framework;
using System.Threading;
using System.Timers;
using System.Diagnostics;
using System.Windows.Forms; //Stopwatch,Debug.WriteLine

 
 

namespace KinectAnimationSample
{
    public class BVHExporter
    {
        StringBuilder sb = new StringBuilder("");
        StringBuilder frames = new StringBuilder("");
        KinectAnimationSample kas;
        Thread thread;
        int frameCnt;

        public BVHExporter(KinectAnimationSample kas)
        {
            this.kas = kas;
            thread = null;
            frameCnt = 0;

        }
        public void putHierarchy()
        {
            
            sb.AppendLine( "HIERARCHY");
            sb.AppendLine( "ROOT Hips");
            sb.AppendLine( "{");
            sb.AppendLine( "	OFFSET	0.00	0.00	0.00");
            sb.AppendLine( "	CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation");
            sb.AppendLine( "	JOINT Chest");
            sb.AppendLine( "	{");
            sb.AppendLine( "		OFFSET	 0.00	 5.21	 0.00");


            //          sb.AppendLine("		OFFSET	 0.00	 " + third + "	 0.00");
            sb.AppendLine( "		CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "		JOINT Neck");
            sb.AppendLine( "		{");
            sb.AppendLine( "			OFFSET	 0.00	 18.65	 0.00");

            //          sb.AppendLine("			OFFSET	 0.00	 " + b + "	 0.00");

            sb.AppendLine( "			CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "			JOINT Head");
            sb.AppendLine( "			{");
            sb.AppendLine( "				OFFSET	 0.00	 3.45	 0.00");


            //          sb.AppendLine("				OFFSET	 0.00	 " + c + "	 0.00");
            sb.AppendLine( "				CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "				End Site");
            sb.AppendLine( "				{");

            sb.AppendLine( "					OFFSET	 0.00	 3.87	 0.00");
            //          sb.AppendLine("					OFFSET	 0.00	 " + Heady + "	 0.00");
            sb.AppendLine( "				}");
            sb.AppendLine( "			}");
            sb.AppendLine( "		}");

            sb.AppendLine( "		JOINT LeftCollar");
            sb.AppendLine( "		{");

            sb.AppendLine( "			OFFSET	 1.12	 16.23	 1.87");
            //          sb.AppendLine("			OFFSET	 " + LeftCollarx + "	" + LeftCollary + "	  " + LeftCollarz);
            sb.AppendLine( "			CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "			JOINT LeftUpArm");
            sb.AppendLine( "			{");
            sb.AppendLine( "				OFFSET	 6.00	 0.00	 0.00");

            //          sb.AppendLine("				OFFSET	 " + d + "	 0.00	 0.00");

            sb.AppendLine( "				CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "				JOINT LeftLowArm");
            sb.AppendLine( "				{");
            sb.AppendLine( "					OFFSET	 0.00	-11.96	 0.00");

            //          sb.AppendLine("					OFFSET	 0.00	" + e + "	 0.00");

            sb.AppendLine( "					CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "					JOINT LeftHand");
            sb.AppendLine( "					{");

            sb.AppendLine( "						OFFSET	 0.00	-9.93	 0.00");

            //          sb.AppendLine("						OFFSET	 0.00	" + f + "	 0.00");
            sb.AppendLine( "						CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "						End Site");
            sb.AppendLine( "						{");

            sb.AppendLine( "							OFFSET	 0.00	-3.00	 0.00");
            //          sb.AppendLine("							OFFSET	 0.00	" + LeftHandy + "	 0.00");
            sb.AppendLine( "						}");
            sb.AppendLine( "					}");
            sb.AppendLine( "				}");
            sb.AppendLine( "			}");
            sb.AppendLine( "		}");
            sb.AppendLine( "		JOINT RightCollar");
            sb.AppendLine( "		{");

            sb.AppendLine( "			OFFSET	-1.12	 16.23	 1.87");
            //          sb.AppendLine("			OFFSET	 " + RightCollarx + "	" + RightCollary + "	  " + RightCollarz);
            sb.AppendLine( "			CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "			JOINT RightUpArm");
            sb.AppendLine( "			{");

            sb.AppendLine( "				OFFSET	-6.00	 0.00	 0.00");

            //          sb.AppendLine("				OFFSET	" + g + "	 0.00	 0.00");

            sb.AppendLine( "				CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "				JOINT RightLowArm");
            sb.AppendLine( "				{");
            sb.AppendLine( "					OFFSET	 0.00	-11.82	 0.00");

            //          sb.AppendLine("					OFFSET	 0.00	" + h + "	 0.00");
            sb.AppendLine( "					CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "					JOINT RightHand");
            sb.AppendLine( "					{");

            sb.AppendLine( "						OFFSET	 0.00	-9.93	 0.00");

            //          sb.AppendLine("						OFFSET	 0.00	" + i + "	 0.00");

            sb.AppendLine( "						CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "						End Site");
            sb.AppendLine( "						{");

            sb.AppendLine( "							OFFSET	 0.00	-3.00	 0.00");
            //         sb.AppendLine("							OFFSET	 0.00	" + RightHandy + "	 0.00");
            sb.AppendLine( "						}");
            sb.AppendLine( "					}");
            sb.AppendLine( "				}");
            sb.AppendLine( "			}");
            sb.AppendLine( "		}");
            sb.AppendLine( "	}");
            sb.AppendLine( "	JOINT LeftUpLeg");
            sb.AppendLine( "	{");

            sb.AppendLine( "		OFFSET	 3.91	 0.00	 0.00");
            //          sb.AppendLine("		OFFSET	 " + LeftUpLegx + "	 0.00	 0.00");

            sb.AppendLine( "		CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "		JOINT LeftLowLeg");
            sb.AppendLine( "		{");

            sb.AppendLine( "			OFFSET	 0.00	-18.34	 0.00");

            //          sb.AppendLine("			OFFSET	 0.00	" +	j +"	 0.00");
            sb.AppendLine( "			CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "			JOINT LeftFoot");
            sb.AppendLine( "			{");

            sb.AppendLine( "				OFFSET	 0.00	-17.37	 0.00");

            //          sb.AppendLine("				OFFSET	 0.00	"+	k +"	 0.00");

            sb.AppendLine( "				CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "				End Site");
            sb.AppendLine( "				{");

            sb.AppendLine( "					OFFSET	 0.00	-3.46	 0.00");
            //          sb.AppendLine("					OFFSET	 0.00	" + LeftFooty + "	 0.00");

            sb.AppendLine( "				}");
            sb.AppendLine( "			}");
            sb.AppendLine( "		}");
            sb.AppendLine( "	}");

            sb.AppendLine( "	JOINT RightUpLeg");
            sb.AppendLine( "	{");

            sb.AppendLine( "		OFFSET	-3.91	 0.00	 0.00");
            //          sb.AppendLine("		OFFSET	 " + RightUpLegx + "	 0.00	 0.00");

            sb.AppendLine( "		CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "		JOINT RightLowLeg");
            sb.AppendLine( "		{");

            sb.AppendLine( "			OFFSET	 0.00	-17.63	 0.00");
            //          sb.AppendLine("			OFFSET	 0.00	" + l + "	 0.00");            

            sb.AppendLine( "			CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "			JOINT RightFoot");
            sb.AppendLine( "			{");

            sb.AppendLine( "				OFFSET	 0.00	-17.14	 0.00");
            //          sb.AppendLine("				OFFSET	 0.00	" + m + "	 0.00");

            sb.AppendLine( "				CHANNELS 3 Zrotation Xrotation Yrotation");
            sb.AppendLine( "				End Site");
            sb.AppendLine( "				{");

            sb.AppendLine( "					OFFSET	 0.00	-3.75	 0.00");
            //          sb.AppendLine("					OFFSET	 0.00	" + RightFooty + "	 0.00");
            sb.AppendLine( "				}");
            sb.AppendLine( "			}");
            sb.AppendLine( "		}");
            sb.AppendLine( "	}");
            sb.AppendLine( "}");
            sb.AppendLine( "MOTION");
        }
        public void startMotionCapture()
        {

            thread = null;
            thread = new Thread(new ThreadStart(recordMotionCaptureLoop));
            thread.Start();

        }
        public void recordMotionCaptureLoop()
        {
           // StreamWriter sw = new StreamWriter(@"d:\test.bvh", true);

            try
            {

                while (true)
                {
                    frameCnt++;
                    // sw.WriteLine("11");
                    putFrame(kas.getskeletonData());

                    Thread.Sleep(40);
                }
            }
            catch (ThreadInterruptedException)
            {
                //save();
               // sw.Close();
                return;
            }
        }

        public void stopMotionCapture()
        {

            if (thread != null)
            {
                thread.Interrupt();
                thread.Join();
                thread = null;
            }
        }

        public void save()
        {
            string filename = @"d:\test.bvh";
            string DefFilename = @"d:\test.bvh";
            SavFileAdv2(DefFilename, ref filename);

            StreamWriter sw = new StreamWriter(filename, false);

            sw.Write(sb.ToString());
            sw.WriteLine("Frames: " + frameCnt);
            sw.WriteLine("Frame Time: 0.0400000000");
            sw.Write(frames.ToString());
            sw.Close();

          //  scrren_save();

        }


        
        private DialogResult SavFileAdv2(string defFilename, ref string filename)
        {
            SaveFileDialog saveFileDialog1 = new SaveFileDialog();
            //saveFileDialog1.InitialDirectory = container.Path;
            saveFileDialog1.FileName = defFilename;
            //saveFileDialog1.DefaultExt = "xml";
            saveFileDialog1.DefaultExt = "bvh";
            //saveFileDialog1.Filter = "XML Files (*.xml) | *.xml";
            saveFileDialog1.Filter = "BVH Files (*.bvh) | *.bvh";

            DialogResult dialogresult = saveFileDialog1.ShowDialog();

            if (dialogresult == System.Windows.Forms.DialogResult.OK)
            {
                filename = saveFileDialog1.FileName;
            }
            return dialogresult;
        }

        private void putFrame(Microsoft.Kinect.Skeleton[] skeleton)
        {
            JointCollection Joints = skeleton[0].Joints;

            float Hipx = Joints[JointType.HipCenter].Position.X;
            float Hipy = Joints[JointType.HipCenter].Position.Y;
            float Hipz = Joints[JointType.HipCenter].Position.Z;
            float Spinx = Joints[JointType.Spine].Position.X;
            float Spiny = Joints[JointType.Spine].Position.Y;
            float Spinz = Joints[JointType.Spine].Position.Z;
            float Neckx = Joints[JointType.ShoulderCenter].Position.X;
            float Necky = Joints[JointType.ShoulderCenter].Position.Y;
            float Neckz = Joints[JointType.ShoulderCenter].Position.Z;
            float Headx = Joints[JointType.Head].Position.X;
            float Heady = Joints[JointType.Head].Position.Y;
            float Headz = Joints[JointType.Head].Position.Z;

            float LeftCollarx = Joints[JointType.ShoulderLeft].Position.X;
            float LeftCollary = Joints[JointType.ShoulderLeft].Position.Y;
            float LeftCollarz = Joints[JointType.ShoulderLeft].Position.Z;
            float LeftUpArmx = Joints[JointType.ElbowLeft].Position.X;
            float LeftUpArmy = Joints[JointType.ElbowLeft].Position.Y;
            float LeftUpArmz = Joints[JointType.ElbowLeft].Position.Z;
            float LeftLowArmx = Joints[JointType.WristLeft].Position.X;
            float LeftLowArmy = Joints[JointType.WristLeft].Position.Y;
            float LeftLowArmz = Joints[JointType.WristLeft].Position.Z;
            float LeftHandx = Joints[JointType.HandLeft].Position.X;
            float LeftHandy = Joints[JointType.HandLeft].Position.Y;
            float LeftHandz = Joints[JointType.HandLeft].Position.Z;

            float RightCollarx = Joints[JointType.ShoulderRight].Position.X;
            float RightCollary = Joints[JointType.ShoulderRight].Position.Y;
            float RightCollarz = Joints[JointType.ShoulderRight].Position.Z;

            float RightUpArmx = Joints[JointType.ElbowRight].Position.X;
            float RightUpArmy = Joints[JointType.ElbowRight].Position.Y;
            float RightUpArmz = Joints[JointType.ElbowRight].Position.Z;
            float RightLowArmx = Joints[JointType.WristRight].Position.X;
            float RightLowArmy = Joints[JointType.WristRight].Position.Y;
            float RightLowArmz = Joints[JointType.WristRight].Position.Z;
            float RightHandx = Joints[JointType.WristRight].Position.X;
            float RightHandy = Joints[JointType.WristRight].Position.Y;
            float RightHandz = Joints[JointType.WristRight].Position.Z;

            float LeftUpLegx = Joints[JointType.HipLeft].Position.X;
            float LeftUpLegy = Joints[JointType.HipLeft].Position.Y;
            float LeftUpLegz = Joints[JointType.HipLeft].Position.Z;
            float LeftLowLegx = Joints[JointType.KneeLeft].Position.X;
            float LeftLowLegy = Joints[JointType.KneeLeft].Position.Y;
            float LeftLowLegz = Joints[JointType.KneeLeft].Position.Z;
            float LeftFootx = Joints[JointType.FootLeft].Position.X;
            float LeftFooty = Joints[JointType.FootLeft].Position.Y;
            float LeftFootz = Joints[JointType.FootLeft].Position.Z;

            float RightUpLegx = Joints[JointType.HipLeft].Position.X;
            float RightUpLegy = Joints[JointType.HipLeft].Position.Y;
            float RightUpLegz = Joints[JointType.HipLeft].Position.Z;
            float RightLowLegx = Joints[JointType.KneeLeft].Position.X;
            float RightLowLegy = Joints[JointType.KneeLeft].Position.Y;
            float RightLowLegz = Joints[JointType.KneeLeft].Position.Z;
            float RightFootx = Joints[JointType.FootLeft].Position.X;
            float RightFooty = Joints[JointType.FootLeft].Position.Y;
            float RightFootz = Joints[JointType.FootLeft].Position.Z;

            float A1 = GetRotationJointsZ(Hipz, Hipy, Hipx);
            float A2 = GetRotationJointsX(Hipx, Hipy, Hipz);
            float A3 = GetRotationJointsY(Hipy, Hipx, Hipz);

            float B1 = GetRotationJointsZ(Spinz, Spiny, Spinx);
            float B2 = GetRotationJointsX(Spinx, Spiny, Spinz);
            float B3 = GetRotationJointsY(Spiny, Spinx, Spinz);

            //float B4 = GetRotationJointsX(Hipx, Hipy, Hipz, Spinx, Spiny, Spinz);
            //float B5 = GetRotationJointsY(Hipx, Hipy, Hipz, Spinx, Spiny, Spinz);
            //float B6 = GetRotationJointsZ(Hipx, Hipy, Hipz, Spinx, Spiny, Spinz);

            float C1 = GetRotationJointsZ(Neckz, Necky, Neckx);
            float C2 = GetRotationJointsX(Neckx, Necky, Neckz);
            float C3 = GetRotationJointsY(Necky, Neckx, Neckz);

            //float C4 = GetRotationJointsX(Spinx, Spiny, Spinz, Neckx, Necky, Neckz);
            //float C5 = GetRotationJointsY(Spinx, Spiny, Spinz, Neckx, Necky, Neckz);
            //float C6 = GetRotationJointsZ(Spinx, Spiny, Spinz, Neckx, Necky, Neckz);

            float D1 = GetRotationJointsZ(Headz, Heady, Headx);
            float D2 = GetRotationJointsX(Headx, Heady, Headz);
            float D3 = GetRotationJointsY(Heady, Headx, Headz);

            //float D4 = GetRotationJointsX(Neckx, Necky, Neckz, Headx, Heady, Headz);
            //float D5 = GetRotationJointsY(Neckx, Necky, Neckz, Headx, Heady, Headz);
            //float D6 = GetRotationJointsZ(Neckx, Necky, Neckz, Headx, Heady, Headz);

            float E1 = GetRotationJointsZ(LeftCollarz, LeftCollarx, LeftCollary);
            float E2 = GetRotationJointsX(LeftCollarx, LeftCollary, LeftCollarz);
            float E3 = GetRotationJointsY(LeftCollary, LeftCollarz, LeftCollarx);

            float F1 = GetRotationJointsZ(LeftUpArmz, LeftUpArmy, LeftUpArmx);
            float F2 = GetRotationJointsX(LeftUpArmx, LeftUpArmy, LeftUpArmz);
            float F3 = GetRotationJointsY(LeftUpArmy, LeftUpArmx, LeftUpArmz);

            //float F7 = GetRotationJointsX(LeftCollarx, LeftCollary, LeftCollarz, LeftUpArmx, LeftUpArmy, LeftUpArmz);
            //float F8 = GetRotationJointsY(LeftCollarx, LeftCollary, LeftCollarz, LeftUpArmx, LeftUpArmy, LeftUpArmz);
            //float F9 = GetRotationJointsZ(LeftCollarx, LeftCollary, LeftCollarz, LeftUpArmx, LeftUpArmy, LeftUpArmz);

            float F4 = GetRotationJointsZ(LeftLowArmz, LeftLowArmy, LeftLowArmx);
            float F5 = GetRotationJointsX(LeftLowArmx, LeftLowArmy, LeftLowArmz);
            float F6 = GetRotationJointsY(LeftLowArmy, LeftLowArmx, LeftLowArmz);

            //float F10 = GetRotationJointsX(LeftUpArmx, LeftUpArmy, LeftUpArmz, LeftLowArmx, LeftLowArmy, LeftLowArmz);
            //float F11 = GetRotationJointsY(LeftUpArmx, LeftUpArmy, LeftUpArmz, LeftLowArmx, LeftLowArmy, LeftLowArmz);
            //float F12 = GetRotationJointsZ(LeftUpArmx, LeftUpArmy, LeftUpArmz, LeftLowArmx, LeftLowArmy, LeftLowArmz);

            float G1 = GetRotationJointsZ(LeftHandz, LeftHandy, LeftHandx);
            float G2 = GetRotationJointsX(LeftHandx, LeftHandy, LeftHandz);
            float G3 = GetRotationJointsY(LeftHandy, LeftHandx, LeftHandz);

            //float G4 = GetRotationJointsX(LeftLowArmx, LeftLowArmy, LeftLowArmz, LeftHandx, LeftHandy, LeftHandz);
            //float G5 = GetRotationJointsY(LeftLowArmx, LeftLowArmy, LeftLowArmz, LeftHandx, LeftHandy, LeftHandz);
            //float G6 = GetRotationJointsZ(LeftLowArmx, LeftLowArmy, LeftLowArmz, LeftHandx, LeftHandy, LeftHandz);

            float H1 = GetRotationJointsZ(RightCollarz, RightCollarx, RightCollary);
            float H2 = GetRotationJointsX(RightCollarx, RightCollary, RightCollarz);
            float H3 = GetRotationJointsY(RightCollary, RightCollarz, RightCollarx);

            float I1 = GetRotationJointsZ(RightUpArmz, RightUpArmy, RightUpArmx);
            float I2 = GetRotationJointsX(RightUpArmx, RightUpArmy, RightUpArmz);
            float I3 = GetRotationJointsY(RightUpArmy, RightUpArmx, RightUpArmz);

            //float I7 = GetRotationJointsX(RightCollarx, RightCollary, RightCollarz, RightUpArmx, RightUpArmy, RightUpArmz);
            //float I8 = GetRotationJointsY(RightCollarx, RightCollary, RightCollarz, RightUpArmx, RightUpArmy, RightUpArmz);
            //float I9 = GetRotationJointsZ(RightCollarx, RightCollary, RightCollarz, RightUpArmx, RightUpArmy, RightUpArmz);

            float I4 = GetRotationJointsZ(RightLowArmz, RightLowArmy, RightLowArmx);
            float I5 = GetRotationJointsX(RightLowArmx, RightLowArmy, RightLowArmz);
            float I6 = GetRotationJointsY(RightLowArmy, RightLowArmx, RightLowArmz);

            //float I10 = GetRotationJointsX(RightUpArmx, RightUpArmy, RightUpArmz, RightLowArmx, RightLowArmy, RightLowArmz);
            //float I11 = GetRotationJointsY(RightUpArmx, RightUpArmy, RightUpArmz, RightLowArmx, RightLowArmy, RightLowArmz);
            //float I12 = GetRotationJointsZ(RightUpArmx, RightUpArmy, RightUpArmz, RightLowArmx, RightLowArmy, RightLowArmz);

            float J1 = GetRotationJointsZ(RightHandz, RightHandy, RightHandx);
            float J2 = GetRotationJointsX(RightHandx, RightHandy, RightHandz);
            float J3 = GetRotationJointsY(RightHandy, RightHandx, RightHandz);

            //float J4 = GetRotationJointsX(RightLowArmx, RightLowArmy, RightLowArmz, RightHandx, RightHandy, RightHandz);
            //float J5 = GetRotationJointsY(RightLowArmx, RightLowArmy, RightLowArmz, RightHandx, RightHandy, RightHandz);
            //float J6 = GetRotationJointsZ(RightLowArmx, RightLowArmy, RightLowArmz, RightHandx, RightHandy, RightHandz);

            float K1 = GetRotationJointsZ(LeftUpLegz, LeftUpLegy, LeftUpLegx);
            float K2 = GetRotationJointsX(LeftUpLegx, LeftUpLegy, LeftUpLegz);
            float K3 = GetRotationJointsY(LeftUpLegy, LeftUpLegx, LeftUpLegz);

            //          float K4 = GetRotationJointsX(RightHandx, RightHandy, RightHandz, LeftUpLegx, LeftUpLegy, LeftUpLegz);
            //          float K5 = GetRotationJointsY(RightHandx, RightHandy, RightHandz, LeftUpLegx, LeftUpLegy, LeftUpLegz);
            //          float K6 = GetRotationJointsZ(RightHandx, RightHandy, RightHandz, LeftUpLegx, LeftUpLegy, LeftUpLegz);

            float L1 = GetRotationJointsZ(LeftLowLegz, LeftLowLegy, LeftLowLegx);
            float L2 = GetRotationJointsX(LeftLowLegx, LeftLowLegy, LeftLowLegz);
            float L3 = GetRotationJointsY(LeftLowLegy, LeftLowLegx, LeftLowLegz);

            //float L4 = GetRotationJointsX(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftLowLegx, LeftLowLegy, LeftLowLegz);
            //float L5 = GetRotationJointsY(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftLowLegx, LeftLowLegy, LeftLowLegz);
            //float L6 = GetRotationJointsZ(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftLowLegx, LeftLowLegy, LeftLowLegz);

            float M1 = GetRotationJointsZ(LeftFootz, LeftFooty, LeftFootx);
            float M2 = GetRotationJointsX(LeftFootx, LeftFooty, LeftFootz);
            float M3 = GetRotationJointsY(LeftFooty, LeftFootx, LeftFootz);

            //float M4 = GetRotationJointsX(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftFootx, LeftFooty, LeftFootz);
            //float M5 = GetRotationJointsY(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftFootx, LeftFooty, LeftFootz);
            //float M6 = GetRotationJointsZ(LeftLowLegx, LeftLowLegy, LeftLowLegz, LeftFootx, LeftFooty, LeftFootz);

            float N1 = GetRotationJointsZ(RightUpLegz, RightUpLegy, RightUpLegx);
            float N2 = GetRotationJointsX(RightUpLegx, RightUpLegy, RightUpLegz);
            float N3 = GetRotationJointsY(RightUpLegy, RightUpLegx, RightUpLegz);

            float O1 = GetRotationJointsZ(RightLowLegz, RightLowLegy, RightLowLegx);
            float O2 = GetRotationJointsX(RightLowLegx, RightLowLegy, RightLowLegz);
            float O3 = GetRotationJointsY(RightLowLegy, RightLowLegx, RightLowLegz);

            //float O4 = GetRotationJointsX(RightUpLegx, RightUpLegy, RightUpLegz, RightLowLegx, RightLowLegy, RightLowLegz);
            //float O5 = GetRotationJointsY(RightUpLegx, RightUpLegy, RightUpLegz, RightLowLegx, RightLowLegy, RightLowLegz);
            //float O6 = GetRotationJointsZ(RightUpLegx, RightUpLegy, RightUpLegz, RightLowLegx, RightLowLegy, RightLowLegz);

            float P1 = GetRotationJointsZ(RightFootz, RightFooty, RightFootx);
            float P2 = GetRotationJointsX(RightFootx, RightFooty, RightFootz);
            float P3 = GetRotationJointsY(RightFooty, RightFootx, RightFootz);

            // sw.WriteLine(" 0  0	 0	 " + A1 + "	 " + A2 + "	 " + A3 + "	 " + B1 + "	 " + B2 + "	 " + B3 + "	 " + C1 + "	 " + C2 + "	 " + C3 + "	 " + D1 + "	 " + D2 + "	 " + D3 + "	 " + E1 + "	 " + E2 + "	 " + 2*E3 + "	 " + F1 + "	 " + F2 + "	 " + F3 + "	 " + F4 + "	 " + F5 + "	 " + F6 + "	 " + G1 + "	 " + G2 + "	 " + G3 + "	 " + H1 + "	 " + H2 + "	 " + H3 + "	 " + I1 + "	 " + I2 + "	 " + I3 + "	 " + I4 + "	 " + I5 + "	 " + I6 + "	 " + J1 + "	 " + J2 + "	 " + J3 + "	 " + K1 + "	 " + K2 + "	 " + K3 + "	 " + L1 + "	 " + L2 + "	 " + L3 + "	 " + M1 + "	 " + M2 + "	 " + M3 + "	 " + N1 + "	 " + N2 + "	 " + N3 + "	 " + O1 + "	 " + O2 + "	 " + O3 + "	 " + P1 + "	 " + P2 + "	 " + P3);
            frames.AppendLine(("\t0.000000\t0.000000\t0.000000\t" 
                + A1 + "\t" + A2 + "\t" + A3 + "\t"
                + B1 + "\t" + B2 + "\t" + B3 + "\t"
                + C1 + "\t" + C2 + "\t" + C3 + "\t"
                + D1 + "\t" + D2 + "\t" + D3 + "\t"
                + (-E1) + "\t" + (-E2) + "\t" + E3 + "\t"
                + F1 + "\t" + F2 + "\t" + F3 + "\t"
                + F4 + "\t" + F5 + "\t" + F6 + "\t"
                + G1 + "\t" + G2 + "\t" + G3 + "\t"
                + (-H1) + "\t" + (-H2) + "\t" + H3 + "\t"
                + I1 + "\t" + I2 + "\t" + I3 + "\t"
                + I4 + "\t" + I5 + "\t" + I6 + "\t"
                + J1 + "\t" + J2 + "\t" + J3 + "\t"
                + K1 + "\t" + K2 + "\t" + K3 + "\t"
                + L1 + "\t" + L2 + "\t" + L3 + "\t"
                + M1 + "\t" + M2 + "\t" + M3 + "\t"
                + N1 + "\t" + N2 + "\t" + N3 + "\t"
                + O1 + "\t" + O2 + "\t" + O3 + "\t"
                + P1 + "\t" + P2 + "\t" + P3));
        }


        /*-------------- Rotation 좌표 획득 함수 --------------*/

        //Microsoft.Xna.Framework.Matrix.CreateFromQuaternion
        public float GetRotationJointsX(float a, float b, float c)
        {
            Vector3 t1 = new Vector3(a, b, c);
            Quaternion q1;

            Microsoft.Xna.Framework.Matrix m1 = Microsoft.Xna.Framework.Matrix.CreateTranslation(t1);
            //q1 = Quaternion.CreateFromRotationMatrix(m1);
            q1 = Quaternion.CreateFromYawPitchRoll(a, b, c);

            double x = q1.X;
            double y = q1.Y;
            double z = q1.Z;
            double w = q1.W;

            double sqx = x * x;
            double sqy = y * y;
            double sqz = z * z;
            double sqw = w * w;

            t1.X = 100 * (float)Math.Asin(2.0f * (q1.Y * q1.Z + q1.W * q1.X));    // roation about x-axis
            //t1.X = (float)(atan2(2.0 * (y * z + x * w), (-sqx - sqy + sqz + sqw)) * 3.14);
            //t1.Y = (float)Math.Atan2(2.0f * (q1.W * q1.Y - q1.X * q1.Z), (-sqx - sqy + sqz + sqw));
            //t1.Z = (float)Math.Atan2(2.0f * (q1.W * q1.Z - q1.X * q1.Y), (-sqx + sqy - sqz + sqw)); 

            return t1.X;
            //return (float)Math.Atan(a / Math.Sqrt(b * b + c * c));
        }

        public float GetRotationJointsY(float a, float b, float c)
        {
            Vector3 t1 = new Vector3(a, b, c);
            Quaternion q1;

            Microsoft.Xna.Framework.Matrix m1 = Microsoft.Xna.Framework.Matrix.CreateTranslation(t1);
            //q1 = Quaternion.CreateFromRotationMatrix(m1);
            q1 = Quaternion.CreateFromYawPitchRoll(a, b, c);

            double x = q1.X;
            double y = q1.Y;
            double z = q1.Z;
            double w = q1.W;

            double sqx = x * x;
            double sqy = y * y;
            double sqz = z * z;
            double sqw = w * w;

            //t1.X = (float)Math.Asin(2.0f * (q1.Y * q1.Z + q1.W * q1.X));    // roation about x-axis
            //t1.X = (float)(atan2(2.0 * (y * z + x * w), (-sqx - sqy + sqz + sqw)) * 3.14);
            t1.Y = 100 * (float)Math.Atan2(2.0f * (q1.W * q1.Y - q1.X * q1.Z), (-sqx - sqy + sqz + sqw));
            //t1.Z = (float)Math.Atan2(2.0f * (q1.W * q1.Z - q1.X * q1.Y), (-sqx + sqy - sqz + sqw));

            return t1.Y;
            //return (float)Math.Atan(a / Math.Sqrt(b * b + c * c));
        }

        public float GetRotationJointsZ(float a, float b, float c)
        {
            Vector3 t1 = new Vector3(a, b, c);
            Quaternion q1;

            Microsoft.Xna.Framework.Matrix m1 = Microsoft.Xna.Framework.Matrix.CreateTranslation(t1);
            //q1 = Quaternion.CreateFromRotationMatrix(m1);
            q1 = Quaternion.CreateFromYawPitchRoll(a, b, c);

            double x = q1.X;
            double y = q1.Y;
            double z = q1.Z;
            double w = q1.W;

            double sqx = x * x;
            double sqy = y * y;
            double sqz = z * z;
            double sqw = w * w;

            //t1.X = (float)Math.Asin(2.0f * (q1.Y * q1.Z + q1.W * q1.X));    // roation about x-axis
            //t1.X = (float)(atan2(2.0 * (y * z + x * w), (-sqx - sqy + sqz + sqw)) * 3.14);
            //t1.Y = (float)Math.Atan2(2.0f * (q1.W * q1.Y - q1.X * q1.Z), (-sqx - sqy + sqz + sqw));
            t1.Z = 100 * (float)Math.Atan2(2.0f * (q1.W * q1.Z - q1.X * q1.Y), (-sqx + sqy - sqz + sqw));

            return t1.Z;
            //return (float)Math.Atan(a / Math.Sqrt(b * b + c * c));
        }


    }   
}